function [FINSSysBlk] = naverrdynmat_inssysblk_psiform(CNE, vN, fN, L, h, G, FCN, C, omegaIE, navCoordType, accoutForGravErr, INSSysStateMask) %#codegen
%NAVERRDYNMAT_INSSYSBLK_PSIFORM 计算ψ形式的导航误差方程对应的误差动态矩阵的系统状态→系统状态块
%
% Input Arguments:
% # CNE: 3*3矩阵，位置矩阵，无单位
% # vN: 3*1向量，速度，单位m/s
% # fN: 3*1向量，比力，单位m/s^2
% # L: 标量，纬度，单位rad
% # h: 标量，高度，单位m
% # G: 标量，引力加速度，单位m/s^2
% # FCN: 3*3矩阵，曲率矩阵，无单位
% # C: 1*3向量，高度通道反馈增益系数，分别反馈至速度二次微分、速度微分、高度微分，均为0或为空时屏蔽高度通道阻尼
% # omegaIE: 标量，地球自转角速率，单位rad/s
% # navCoordType: 枚举标量，NavCoordType.GEO为ENU地理坐标系，NavCoordType.WAN_AZM为游动方位坐标系，NavCoordType.FREE_AZM为自由方位坐标系
% # accoutForGravErr: 逻辑标量，true表示考虑重力误差
% # INSSysStateMask: 1*11逻辑向量，true表示包含该系统状态，false表示不包含该系统状态，详细状态为第1、2、10列元素为位置误差，第3列元素为高度误差，第4-6列元素为E系下观测的速度误差在N系下的分量，第7-9列元素为姿态误差，第11列为deVC3
%
% Output Arguments:
% # FINSSysBlk: m*m矩阵，m为INSSysStateMask中为真的个数，误差动态矩阵的系统状态→系统状态块，无单位
%
% Assumptions and Limitations:
% # 理论上，传入本函数的导航参数应为理想值
%
% References:
% # 《应用导航算法工程基础》“地球系下观测的姿态、速度误差和导航系下观测的位置误差投影在导航系下的参数组”

%% 预计算
tanL = tan(L);
tanLSquarePlusOne = tanL^2 + 1;
RMPlushReciprocal = coder.nullcopy(NaN);
RNPlushReciprocal = coder.nullcopy(NaN);
RMPlushRecSquare = coder.nullcopy(NaN);
RNPlushRecSquare = coder.nullcopy(NaN);
R0PlushReciprocal = coder.nullcopy(NaN);
R0PlushRecSquare = coder.nullcopy(NaN);
if navCoordType == NavCoordType.GEO
    % REF1式9.105
    RMPlushReciprocal = FCN(1, 1);
    RNPlushReciprocal = FCN(2, 2);
    RMPlushRecSquare = RMPlushReciprocal^2;
    RNPlushRecSquare = RNPlushReciprocal^2;
else
    % REF1第9.1.6节
    R0PlushReciprocal = (FCN(1, 1) + FCN(2, 2))/2;
    R0PlushRecSquare = R0PlushReciprocal^2;
end

%% 状态块中各分量式
omegaIEN = earthrate(CNE, omegaIE);
omegaENN = FCN * [-vN(2, 1) vN(1, 1) 0]'; % 等号右边后一项为[0 0 1]'×vN
if navCoordType == NavCoordType.GEO
    omegaENN(3, 1) = tanL * omegaENN(2, 1);
elseif navCoordType == NavCoordType.FREE_AZM
    omegaENN(3, 1) = -omegaIEN(3, 1);
end
omegaIENx = omegaIEN(1, 1); 
omegaIENy = omegaIEN(2, 1);
omegaIENz = omegaIEN(3, 1);
omegaENNx = omegaENN(1, 1);
omegaENNy = omegaENN(2, 1);
omegaENNz = omegaENN(3, 1);
vNx = vN(1, 1);
vNy = vN(2, 1);
vNz = vN(3, 1);
fNx = fN(1, 1);
fNy = fN(2, 1);
fNz = fN(3, 1);

%% 计算各状态块
FINSSysBlkFull = zeros(11);
switch navCoordType
    case NavCoordType.GEO
        FINSSysBlkFull(1:10, 1:10) =    [                        -vNz*RMPlushReciprocal,                           omegaENNz,           vNy*RMPlushRecSquare,                         0,             -1*RMPlushReciprocal,                         0,                       0,                     0,                       0, vNx*RMPlushReciprocal - omegaENNy ;
                                                        -omegaENNz,                       -vNz*RNPlushReciprocal,          -vNx*RNPlushRecSquare,                1*RNPlushReciprocal,                       0,                         0,                       0,                     0,                       0, omegaENNx + vNy*RNPlushReciprocal ;
                                                              -vNy,                                 vNx,                        0,                         0,                       0,                         1,                       0,                     0,                       0,                        0 ;
                                                                 0,                                   -G,                        0,                         0, 2*omegaIENz + omegaENNz, - 2*omegaIENy - omegaENNy,                       0,                  -fNz,                     fNy,                        0 ;
                                                                 G,                                   0,                        0, - 2*omegaIENz - omegaENNz,                       0,                 omegaENNx,                     fNz,                     0,                    -fNx,                        0 ;
                                                                 0,                                   0,                        0,   2*omegaIENy + omegaENNy,              -omegaENNx,                         0,                    -fNy,                   fNx,                       0,                        0 ;
                                                                 0,                                   0,                        0,                         0,                       0,                         0,                       0, omegaIENz + omegaENNz, - omegaIENy - omegaENNy,                        0 ;
                                                                 0,                                   0,                        0,                         0,                       0,                         0, - omegaIENz - omegaENNz,                     0,               omegaENNx,                        0 ;
                                                                 0,                                   0,                        0,                         0,                       0,                         0,   omegaIENy + omegaENNy,            -omegaENNx,                       0,                        0 ;
                              omegaENNy - omegaENNy*tanLSquarePlusOne, - omegaENNx - (vNz*tanL)*RNPlushReciprocal, -(vNx*tanL)*RNPlushRecSquare,           tanL*RNPlushReciprocal,                       0,                         0,                       0,                     0,                       0,    (vNy*tanL)*RNPlushReciprocal];
     case NavCoordType.WAN_AZM
        FINSSysBlkFull(1:10, 1:10) =    [ -vNz*R0PlushReciprocal,             0,  vNy*R0PlushReciprocal^2,                       0,               -1*R0PlushReciprocal,                         0,                     0,                       0,                       0, 0 ;
                                          0, -vNz*R0PlushReciprocal, -vNx*R0PlushReciprocal^2,              1*R0PlushReciprocal,                         0,                         0,                     0,                       0,                       0, 0 ;
                                       -vNy,           vNx,               0,                       0,                         0,                         1,                     0,                       0,                       0, 0 ;
                                          0,             -G,               0,                       0,               2*omegaIENz, - 2*omegaIENy - omegaENNy,                     0,                    -fNz,                     fNy, 0 ;
                                          G,             0,               0,            -2*omegaIENz,                         0,   2*omegaIENx + omegaENNx,                   fNz,                       0,                    -fNx, 0 ;
                                          0,             0,               0, 2*omegaIENy + omegaENNy, - 2*omegaIENx - omegaENNx,                         0,                  -fNy,                     fNx,                       0, 0 ;
                                          0,             0,               0,                       0,                         0,                         0,                     0,               omegaIENz, - omegaIENy - omegaENNy, 0 ;
                                          0,             0,               0,                       0,                         0,                         0,            -omegaIENz,                       0,   omegaIENx + omegaENNx, 0 ;
                                          0,             0,               0,                       0,                         0,                         0, omegaIENy + omegaENNy, - omegaIENx - omegaENNx,                       0, 0 ;
                                  omegaENNy,    -omegaENNx,               0,                       0,                         0,                         0,                     0,                       0,                       0, 0];
     case NavCoordType.FREE_AZM
        FINSSysBlkFull(1:10, 1:10) =    [         -vNz*R0PlushReciprocal,              -omegaIENz,  vNy*R0PlushRecSquare,                       0,               -1*R0PlushReciprocal,                         0,                     0,                       0,                       0, 0 ;
                                          omegaIENz,           -vNz*R0PlushReciprocal, -vNx*R0PlushRecSquare,              1*R0PlushReciprocal,                         0,                         0,                     0,                       0,                       0, 0 ;
                                               -vNy,                     vNx,               0,                       0,                         0,                         1,                     0,                       0,                       0, 0 ;
                                                  0,                       -G,               0,                       0,                 omegaIENz, - 2*omegaIENy - omegaENNy,                     0,                    -fNz,                     fNy, 0 ;
                                                  G,                       0,               0,              -omegaIENz,                         0,   2*omegaIENx + omegaENNx,                   fNz,                       0,                    -fNx, 0 ;
                                                  0,                       0,               0, 2*omegaIENy + omegaENNy, - 2*omegaIENx - omegaENNx,                         0,                  -fNy,                     fNx,                       0, 0 ;
                                                  0,                       0,               0,                       0,                         0,                         0,                     0,                       0, - omegaIENy - omegaENNy, 0 ;
                                                  0,                       0,               0,                       0,                         0,                         0,                     0,                       0,   omegaIENx + omegaENNx, 0 ;
                                                  0,                       0,               0,                       0,                         0,                         0, omegaIENy + omegaENNy, - omegaIENx - omegaENNx,                       0, 0 ;
                              omegaIENy + omegaENNy, - omegaIENx - omegaENNx,               0,                       0,                         0,                         0,                     0,                       0,                       0, 0];
    otherwise
        FINSSysBlkFull = NaN(11); %#ok<NASGU>
        error('naverrdynmat_inssysblk_psiform:navCoordType', 'Invalid navigation coordinate type.');
end
if any(C) 
    % 考虑高度通道阻尼
    FINSSysBlkFull(3, 3) = FINSSysBlkFull(3, 3) - C(1, 3);
    FINSSysBlkFull(6, 3) = FINSSysBlkFull(6, 3) - C(1, 2);
    FINSSysBlkFull(11, 3) = C(1, 1);
    FINSSysBlkFull(6, 11) = -1;
end
if accoutForGravErr 
    % 考虑重力误差
    % REF1式9.72
    if h>=0
        Fh = 2;
    else
        Fh = -1;
    end    
    FINSSysBlkFull(6, 3) = FINSSysBlkFull(6, 3) + Fh*G*(FCN(1, 1)+FCN(2, 2))/2; % NOTE: 此处用R0+h近似代替载体所在处的地球半径R
end
FINSSysBlk = FINSSysBlkFull(INSSysStateMask, INSSysStateMask);
end